/*
 * Dalek.h
 *
 *  Created on: Feb 29, 2012
 *      Author: baneling
 */

#ifndef _DALEK_H_
#define _DALEK_H_
#include "includes.h"
#include "Pipe.h"
#include <libplayerc++/playerc++.h>
#include <ctime>
#include <list>

using namespace PlayerCc;
using namespace std;

#define PINK 16761035
#define ORANGE 16753920
#define BLUE 255
#define RADIUSCONSTANTCONVERSION 57.2957796
#define OFFPOSITIONTH 0.3
#define SOGLIASVOLTE 500

#define SEND_TO_ALL "everyone"

enum direction_t {top,bot,dx,sx};
enum position_t {anterior,posterior,righth,lefth};
enum quadrant_t {lowleft,lowright,hileft,hiright};
enum orientation_t {noaction,correctposition,turnyaw,formationoriented};
enum states_t { WANDERING, FORMATION, FORMATIONFARAWAY, WAITINFORMATION,STOP,WAITINFORMANSWER, IFOUNDDR,IFOUNDCAB,DOCFOUND,CABFOUND,CHECKDRPOSITION, SENSORTEST,SEENDOCTOR,SEENCABIN,WAITOTHERSAWAY,FORMATIONORIENTATION,FORMATIONORIENTED,FORMATIONGO,WAITINGFORFORMATION, PaskTest};//todo leva pasktest
//in caso di modifiche alla lista di stati (anche cambio di posizione!) aggiornare la ricezione del messaggio tipo ansState!!!!
struct Item
{
	string name;
	double x;
	double y;
}typedef item_t;

struct PACKET{
	msg_t message;
	int receiver_id;
	bool isRead;
}typedef packet_t;

struct cordinates_s
{
	double x;
	double y;
	double yaw;
}typedef coordinates_t;

struct otherstates_s
{
	states_t state;
	string name;
	bool inExpQuadrant;
}typedef otherstates_t;

struct obstacles_s
{
	bool sensorError;
	int can;
	int jast;
	int thay;
	int sec;
	bool _90;
	bool _180;
	bool _270;
	bool _0;
}typedef obstacles_t;

struct dalekPipe_s
{
	int pipeId;
	string dalekReader;
	string dalekWriter;
}typedef dalekPipe_t;

class Dalek {
	public:
		Dalek(string name,string address, uint32_t positionProxyIndex,uint32_t sonarProxyIndex,uint32_t bloblfinderProxyIndex, uint32_t simulationProxyIndex);
		virtual ~Dalek();
		/*other public methods*/
		void run(); //N.B. while(true) method
		void writeOnPipe(enum message_t mess,string receiver,double x, double y);
		bool readOnStatePipe();
		void writeOnStatePipe(enum message_t mess,string receiver,double x, double y);
		bool readOnPipe();
		//void setPipes(int pWrite,int pRead);
		void setPipes(int pWrite,int pRead,string pReader);
		void setStatePipes(int pWrite,int pRead);
		string getName(){return this->dalekName;};

	private: /*private methods*/
		void wander ();
		void avoidObstacles(double *forwardSpeed, double *turnSpeed, SonarProxy &sp, bool pushing);
		void moveToItem(uint32_t targetcolor);
		int FindItem();
		void RefreshItemList();
		void handleTarget(bool *impactFlag, bool *pushing);
		void running(string str);
		bool countColorBlob(uint32_t color,BlobfinderProxy &bfp);
		void reachLocation(double targetX, double targetY,SimulationProxy &simProxy,double temperature, double *forwardSpeed, double *turnSpeed);
		void wanderingState(bool foundDrFlag,uint32_t targetcolor,bool push,double temperature);
		double getFormationCoord(direction_t dir, bool coordIsX);
		void reachFormationPlace(SimulationProxy &simProxy,double forwardSpeed, double turnSpeed);
		void askShiftInFormation(string receiver, double x, double y);
		void askStateFunction(string receiver);
		void updateStateFunction(string sender, double statex);
		void broadcastStateFunction(states_t s);
		void getNextFormationPos(double* x,double* y);
		bool checkClearPath(double rotDist, double margin);
		void checkDoctor();
		void resetPacket();
		void avoidObstaclesWithTarget();
		bool watchAround(uint32_t targetcolor);
		bool circumnavigateDr();
		bool isInTheAround(coordinates_t toCheck, coordinates_t theAround);
		bool areSensorsLeftFree(double frontsx=0.4,double frontdx=0.3,double lateral=0.1,double anterior=0.1,double diagonal=0.2);
		void moveDr();
		void checkCabin();
		bool goAwayFromDoctor(double distanceneed=2.5);
		bool noneIsAwain();
		void navigate ();
		int easyNavigate(int counter);
		void stopFormation();
		bool turnToYaw(double yaw,double errormargin=2);
		bool turnToYawGlobalSafe(double yaw,double errormargin=2);
		bool checkEveryoneState(states_t state1,states_t state2);
		string checkEveryoneState(states_t state1);
		int formationKeeper();
		void correctAngle(double * turnSpeed);
		void checkRole();
		bool checkObstacles();
		bool naviCheckDir(double nextYaw);
		obstacles_t areSensorsAnteriorFree(int cfrYaw);
		obstacles_t areSensorsFormationPosteriorFree(int cfrYaw);
		obstacles_t initObstaclesPosition(obstacles_t obt);
		bool checkObstaclesFlag(obstacles_t obt);
		int isLateOrTooForward(double tempcoord, position_t role);
		obstacles_t areSensorsFormationLeftFree(int cfrYaw);
		obstacles_t areSensorsFormationRightFree(int cfrYaw);
		string roleToString(int role);
		void goStraight(double vel);
		bool stopInCorrectPosition();
		string stateToString( double statex);
		int angleDiff(double ax,double ay,double ayaw, double bx,double by);
		double yawToTurnTo(double ax,double ay, double bx,double by);
		double getRandomRotationSpeed(int startFrom, int widthInterval);
		void getDalekDir();
		direction_t getNextDir();
		int getDalek90Yaw();
		int checkObstaclesYaw();
		int getRandomDirection();
	private: /* attributes*/
		PlayerClient *robotClient;
		Position2dProxy *p2dProxy;
		SonarProxy *sonarProxy;
		BlobfinderProxy *blobProxy;
		SimulationProxy *simProxy;
		string dalekName;
		item_t itemList;
		msg_t message;
		//list<dalekPipe_t> pipeRead;
		list<dalekPipe_t> pipeWrite;
		list <int> pipeRead;
		//list <int> pipeWrite;
		list <int> pipeStateRead;
		list <int> pipeStateWrite;
		list <otherstates_t> dalekNames;
		int tempcounter;
		list <packet_t> packetRead;
		list <packet_t> packetStateRead;
		playerc_blobfinder_blob_t blobFound;
		states_t state;
		states_t prevState;
		coordinates_t docCoord;
		coordinates_t cabCoord;
		coordinates_t formationPlace;
		coordinates_t myCoord;
		coordinates_t startingCoord;
		quadrant_t dalekQuadrant;
		int waitinformAnsCounter;
		int reachlocationCounter;
		int aroundCount;
		int svolteCount;
		//int SOGLIASVOLTE;
		int circumnavigateCount;
		int allontanamentoCount;
		int localminimaCount;

		double drDistance;
		double forwardspeed;
		double turnspeed;
		double bestsofar;
		bool avoiding;
		bool imNavigator;
		bool drFree;
		double formationYaw;
		double oldFormationYaw;
		position_t formationPosition;
		obstacles_t obstaclesPosition;
		bool stopFlag;
		double wantedYaw;
		int counterYaw;
		bool pushDr;
		bool nearDoc;
		bool iCanPush;

		orientation_t formationOrientationFlag;
		direction_t mydir;
};

#endif /* DALEK_H_ */
